/**

Title   : ControlThread.cpp
Author  : Manikantan Nambi
URL :

Description : Function definitions for ControlThread.h
Created : 1/1/11
Modified   : 2/14/11

*/


#include <src/ControlThread.h>
extern CvPoint probeTip;
extern double velmag;
extern double veldir;

/*
*   Summary:    ControlThread class constructor
*
*   Parameters:     parent Thread
*   Returns:    None
*/
ControlThread::ControlThread(QObject *parent, Joystick *joystick):QThread(parent)
{

    // controlTimer = new QTimer();

    // controlTimer->setInterval(10);

    // connecting timer to MM3A path planning algorithm
    //connect(controlTimer, SIGNAL(timeout()), this, SLOT(run()));

    //controlTimer->start();

    mm3a = new MM3AWidget(); // initialize mm3a object

    tempJoystick = joystick; // storing joystick in controlThread variable

    mode = 1; // joystick mode is selected by default

    // setting values of corners in pixels
    corners[0][0] = 50;
    corners[0][1] = 50;

    corners[1][0] = 590;
    corners[1][1] = 50;

    corners[2][0] = 590;
    corners[2][1] = 430;

    corners[3][0] = 50;
    corners[3][1] = 430;


    qDebug()<<"ControlThread: ControlThread():Initializing...Done";
}

void ControlThread::open()
{

}

/*
*   Summary:    function to perform tasks in ControlThread
*
*   Parameters:     None
*   Returns:    None
*/
void ControlThread::run() {

    while(isRunning())
    {
        int servoTo = 0;
        switch(cornerCount) {
        case 0:
            servoTo = 1;
            break;
        case 1:
            servoTo = 2;
            break;
        case 2:
            servoTo = 3;
            break;
        case 3:
            servoTo = 0;
            break ;
        }

        if(mode==0) {
            // experiment mode
            int x = corners[servoTo][0] ;
            int y = corners[servoTo][1] ;

            Real threshold =  (x-probeTip.x)*(x-probeTip.x) + (y-probeTip.y)*(y-probeTip.y);

            if(sqrt(threshold) > 10) {
                velMag = 0.01;
                velDir = atan2((y-probeTip.y),(x-probeTip.x) );
                mm3a->variableStepMag(velMag, velDir);
                // qDebug()<< "Redirected velocity"<<velMag<< velDir << cos(velDir) << probeTip.x << probeTip.y;
            } else {
                if(cornerCount<4) {
                    cornerCount++;
                    mm3a->setEstCorner(cornerCount);
                } else {
                    mm3a->setEstCorner(cornerCount);
                    sleep(1);
                    quit();

                }
            }
        } else {
            if(spaceExp) {
                mm3a->variableStepMag(tempJoystick->velMag()*0.018/(162* 1), tempJoystick->velDir());
//                qDebug()<< __LINE__ << Q_FUNC_INFO << "velmag = " << tempJoystick->velMag();

            } else {
                // qDebug()<<"Phantom velMag"<<velmag;
                mm3a->variableStepMag(velmag * 0.018 / 50, veldir);
            }
        }

        // mm3a->variableStepMag(0.05, 3.14/6 );
        //qDebug()<<"Omni vel Mag = "<<velmag;
        msleep(10);
    }
    qDebug() << __LINE__ << Q_FUNC_INFO;

    exec();

}

/*
*   Summary:    close funtion
*
*   Parameters:     None
*   Returns:    None
*/
void ControlThread::close()
{
    controlTimer->stop();
    // mm3a->close();
    // delete mm3a;
    // mm3a = NULL;
    qDebug()<<"\nDeleted MM3A widget";

}

void ControlThread::reset()
{
    mm3a->close();
    delete mm3a;

    mm3a = new MM3AWidget();
}

void ControlThread::simulate()
{

    switch(mm3a->simulate_status) {
    case 0:
        mm3a->simulate_status = 1;
        qDebug()<< "MM3AWidget::simulate():simulate_status"<<mm3a->simulate_status;
        break;

    case 1:
        mm3a->simulate_status = 0;
        qDebug()<< "MM3AWidget::simulate():simulate_status"<<mm3a->simulate_status;
        break;
    }
}
